#include "launcher.h"
#include "../Commands/LauncherJoy.h"
#include "../CommandBase.h"

Launcher::Launcher() : Subsystem("launcher")
{	
	bottomJaguar = new CANJaguar(LAUNCHER_BOTTOM_JAGUAR, CANJaguar::kSpeed);
	topJaguar = new CANJaguar(LAUNCHER_TOP_JAGUAR, CANJaguar::kSpeed);
	bottomJaguar->ConfigEncoderCodesPerRev(2048);
	topJaguar->ConfigEncoderCodesPerRev(2048);
	bottomJaguar->SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
	topJaguar->SetSpeedReference(CANJaguar::kSpeedRef_QuadEncoder);
	bottomJaguar->EnableControl(0);
	topJaguar->EnableControl(0);
	bottomJaguar->SetVoltageRampRate(50);
	topJaguar->SetVoltageRampRate(50);
	bottomJaguar->SetPID(1000,0,0);
	topJaguar->SetPID(1000,0,0);
}

void Launcher::launcherJoystick(Joystick *joystick)
{
	bottomJaguar->Set(joystick->GetY()/7.4*90);
	topJaguar->Set(joystick->GetY()/7.4*90);
}

void Launcher::InitDefaultCommand()
{
	SetDefaultCommand(new LauncherJoy());
}
